import logging
from logging.handlers import RotatingFileHandler
from fairino import Robot
import time
import numpy as np

class IndustrialRobotController:
    def __init__(self, ip_address='192.168.58.2'):
        # 初始化日志系统
        self._init_logging()
        self.logger = logging.getLogger('RobotController')
        
        # 系统配置
        self._ip = ip_address
        self._robot = None
        self.home_position = [4.275, -95.327, 3.751, -8.896, 159.276, 162.871]
        self.position_tolerance = 1.0
        self.home_tolerance = 2.0  # home点校验容差（单位：度）
        # 状态监控
        self._operation_status = {
            'online': False,
            'in_motion': False,
            'current_location': 'unknown',
            'last_error': (-1, -1),
            'collision_strategy': None
        }

        # 初始化连接
        try:
            self._initialize_controller()
            self.set_crash_level(1)  # 默认碰撞后继续运动
        except Exception as e:
            self.logger.critical("控制器初始化失败", exc_info=True)
            raise

    def _init_logging(self):
        """配置日志系统"""
        logging.basicConfig(
            level=logging.DEBUG,
            format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
            handlers=[
                RotatingFileHandler(
                    'robot_operation.log',
                    maxBytes=10 * 1024 * 1024,
                    backupCount=5,
                    encoding='utf-8'
                ),
                logging.StreamHandler()
            ]
        )
        logging.captureWarnings(True)

    def _initialize_controller(self):
        """控制器初始化"""
        self.logger.info("正在初始化控制器...")
        try:
            self._robot = Robot.RPC(self._ip)
            self.logger.debug(f"已创建RPC连接到 {self._ip}")

            error_code, data = self._robot.GetRobotErrorCode()
            if error_code != 0:
                raise ConnectionError(f"初始握手失败，错误码: {error_code}")
                
            self._parse_error_code(data)
            self._operation_status['online'] = True
            self.logger.info("控制器初始化成功")
            
        except Exception as e:
            self.logger.critical("控制器初始化异常", exc_info=True)
            raise

    def _parse_error_code(self, code_data):
        """解析错误码"""
        if len(code_data) >= 2:
            main, sub = code_data[0], code_data[1]
            self._operation_status['last_error'] = (main, sub)
            self.logger.warning(f"当前错误状态 主码={main}, 子码={sub}") if main !=0 else None

    # ---------- 关键功能方法 ----------
    def execute_work_cycle(self):
        """完整工作周期"""
        self.logger.info("启动工作周期")
        try:
            if not self._precheck_system():
                return False
                
            if not self._return_to_safe():
                self.logger.warning("启动前安全位置校验失败")
                return False

            if not self._move_to_target("welding_point"):
                return False

            self.logger.info("执行焊接工艺(模拟10秒)")
            time.sleep(10)

            return self._return_to_safe()
            
        except Exception as e:
            self.logger.error("工作周期异常", exc_info=True)
            return False
        finally:
            self._post_cycle_check()
            self.logger.info("工作周期结束")

    def _precheck_system(self):
            """系统预检（增强版）"""
            self.logger.debug("执行系统预检")
            
            # 1. 连接状态检查
            if not self._check_connection():
                self.logger.error("预检失败：设备离线")
                return False
                
            # 2. 运动状态检查
            motion_state = self._get_motion_state()
            if motion_state['error'] is not None:
                self.logger.error(f"运动状态查询失败，错误码: {motion_state['error']}")
                return False
            if motion_state['in_motion']:
                self.logger.warning("预检失败：设备正在运动中")
                return False
                
            # 3. 位置状态检查（新增）
            position_status = self._check_home_position()
            if not position_status['is_valid']:
                self.logger.error("预检失败：位置校验异常")
                return False
            if not position_status['is_at_home']:
                self.logger.warning("预检失败：设备未在原点")
                return False
                
            self.logger.debug("预检通过")
            return True

    def _check_home_position(self):
        """
        位置状态检测
        返回: {
            'is_valid': bool,   # 是否成功获取有效位置
            'is_at_home': bool, # 是否在home点
            'deviations': list  # 各关节偏差值
        }
        """
        result = {
            'is_valid': False,
            'is_at_home': False,
            'deviations': []
        }
        
        try:
            error_code, positions = self.get_current_joint_pos()
            if error_code != 0:
                self.logger.warning(f"位置读取失败，错误码: {error_code}")
                return result
                
            # 计算各关节偏差
            deviations = np.abs(np.array(positions) - self.home_position)
            max_deviation = np.max(deviations)
            
            # 更新状态字典
            self._operation_status['current_location'] = 'home' if max_deviation <= self.home_tolerance else 'unknown'
            
            result.update({
                'is_valid': True,
                'is_at_home': max_deviation <= self.home_tolerance,
                'deviations': deviations.tolist()
            })
            
            self.logger.debug(f"位置偏差检测结果: {deviations}")
        except Exception as e:
            self.logger.error("位置检测异常", exc_info=True)
            
        return result

    # 新增状态获取方法
    def get_current_location(self):
        """
        获取当前设备位置状态
        返回: {
            'status': 'home'/'work'/'moving'/'unknown',
            'deviations': list,
            'max_deviation': float
        }
        """
        status = {
            'status': self._operation_status['current_location'],
            'deviations': [],
            'max_deviation': 0.0
        }
        
        if self._operation_status['in_motion']:
            status['status'] = 'moving'
            return status
            
        pos_status = self._check_home_position()
        if pos_status['is_valid']:
            status.update({
                'deviations': pos_status['deviations'],
                'max_deviation': np.max(pos_status['deviations']) if pos_status['deviations'] else 0.0
            })
            
        return status

    def _check_connection(self):
        """连接状态检查"""
        try:
            error_code, data = self._robot.GetRobotErrorCode()
            if error_code == 0:
                self._parse_error_code(data)
                return True
            self.logger.warning(f"连接错误码: {error_code}")
            return False
        except Exception as e:
            self.logger.error("连接检查异常", exc_info=True)
            return False

    def _get_motion_state(self):
        """获取运动状态"""
        try:
            error_code, ret = self._robot.GetRobotMotionDone()
            if error_code == 0:
                self._operation_status['in_motion'] = ret == 0
                return {'in_motion': ret == 0, 'error': None}
            self.logger.warning(f"状态查询错误码: {error_code}")
            return {'in_motion': False, 'error': error_code}
        except Exception as e:
            self.logger.error("状态查询异常", exc_info=True)
            return {'in_motion': False, 'error': -1}

    def _move_to_target(self, point_name):
        """移动到目标点"""
        self.logger.info(f"向 {point_name} 移动")
        target = [10.5, -75.3, 105.8, -15.2, 170.5, 160.0]
        
        try:
            error_code = self._robot.MoveJ(target, tool=1, user=0, vel=30)
            if error_code != 0:
                self._handle_motion_error(error_code)
                return False
            return self._confirm_movement()
        except Exception as e:
            self.logger.error("移动异常", exc_info=True)
            return False

    def _confirm_movement(self, timeout=10):
        """运动确认"""
        start = time.time()
        while time.time() - start < timeout:
            state = self._get_motion_state()
            if not state['in_motion']:
                return True
            time.sleep(0.5)
        self.logger.error(f"运动超时 ({timeout}s)")
        return False

    def _return_to_safe(self):
        """安全返回"""
        self.logger.info("启动安全返回")
        try:
            error_code = self._robot.MoveJ(self.home_position, tool=1, user=0, vel=30)
            if error_code != 0 or not self._confirm_movement(15):
                return False
            return self._update_home_status()
        except Exception as e:
            self.logger.critical("安全返回异常", exc_info=True)
            return False

    def _update_home_status(self):
        """更新home点状态"""
        if self.is_at_home():
            self._operation_status['current_location'] = 'home'
            self.logger.info("精确返回安全位置")
            return True
        self.logger.warning("位置偏差超出容差")
        return False

    def _post_cycle_check(self):
        """周期后安全检查"""
        self.logger.debug("执行周期后检查")
        try:
            if not self._operation_status.get('at_home', False):
                self.logger.warning("设备未在安全位置!")
                self._emergency_stop()
        except Exception as e:
            self.logger.error("周期后检查异常", exc_info=True)

    def _emergency_stop(self):
        """紧急停止"""
        self.logger.critical("触发紧急停止!")
        try:
            ret = self._robot.StopMotion()
            if ret == 0:
                self.logger.warning("急停指令已发送")
            else:
                self.logger.error(f"急停失败 错误码: {ret}")
        except Exception as e:
            self.logger.critical("急停异常", exc_info=True)

    # ---------- 公共接口 ----------
    def set_crash_level(self, block=1):
        """设置碰撞策略 (0:停止 1:继续)"""
        try:
            error_code = self._robot.SetCollisionStrategy(block)
            if error_code == 0:
                self._operation_status['collision_strategy'] = block
                self.logger.info(f"碰撞策略设置为: {'继续运动' if block else '停止'}")
                return True
            self.logger.error(f"设置失败 错误码: {error_code}")
            return False
        except Exception as e:
            self.logger.error("策略设置异常", exc_info=True)
            return False

    def get_current_joint_pos(self):
        """获取当前关节角度"""
        try:
            error_code, pos = self._robot.GetActualJointPosDegree()
            return (error_code, [round(p,3) for p in pos]) if error_code ==0 else (error_code, None)
        except Exception as e:
            self.logger.error("获取角度异常", exc_info=True)
            return (-1, None)

    def is_at_home(self):
        """校验是否在home点"""
        error_code, pos = self.get_current_joint_pos()
        if error_code != 0: 
            return False
        return np.all(np.abs(np.array(pos) - self.home_position) <= self.position_tolerance)

    def get_position_status(self):
        """获取完整位置状态"""
        status = {
            'position': None,
            'is_at_home': False,
            'deviations': None
        }
        error_code, pos = self.get_current_joint_pos()
        if error_code == 0:
            status.update({
                'position': pos,
                'deviations': np.abs(np.array(pos) - self.home_position).tolist(),
                'is_at_home': self.is_at_home()
            })
        return status

if __name__ == "__main__":
    controller = IndustrialRobotController()
    if controller.execute_work_cycle():
        print("工作周期完成，最终状态:", controller.get_position_status())
    else:
        print("工作周期异常终止")
    print("碰撞策略状态:", controller._operation_status['collision_strategy'])